/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Test;
import Behaviors.Arbitrators.*;
import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Robot.*;
import Robot.Sensors.*;
import Utils.*;

import javaclient3.*;
import javaclient3.structures.*;
/**
 *
 * @author alejo
 */
public class VanishPoint {
    
    public static void main(String args[])
    {

        boolean isSimulated = false;
        boolean isUsingVp = true;
        boolean showArbFrame = true;
        boolean showMapObstFrame = true;


        Robot real;

        //Creacion de la instancia robot.
        if(isSimulated)
        {
            real = new Robot("real","localhost",6665, 0);
            //camera = new Robot("192.168.1.123",6665,"camera",1);
        }
        else
        {
            real = new Robot("real","192.168.0.1",6665, 0);
            //camera = new Robot("192.168.1.123",6665,"camera",1);
        }

        Position2DInterface posReal;
        posReal = real.requestInterfacePosition2D(0, PlayerConstants.PLAYER_OPEN_MODE);

        Position2DHelper posH1 = new Position2DHelper();
        if(isSimulated)
        {
            posH1.setMaxSpeed(0.4f);
            posH1.setRotControl(0.3f,1.0f);
        }
        else
        {
            posH1.setMaxSpeed(1.0f);
            posH1.setRotControl(0.2f,0.2f);
        }


        PlayerDevice ir;

        try{
            if(isSimulated)
                ir = real.requestInterfaceSonar(0, PlayerConstants.PLAYER_OPEN_MODE);
            else
                ir = real.requestInterfaceIR(0, PlayerConstants.PLAYER_OPEN_MODE);

    
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }

           
        //VAnish point sensor
        VPSensor vp = new VPSensor(640,480,"localhost",25557);
        if(isUsingVp)
            vp.start();

        real.runThreaded(-1, -1);

        posReal.setMotorPower(1);

        Pose2d initialPose;
        if(isSimulated)
            initialPose = new Pose2d((float)15,(float)25,-(float)Math.PI/2);
        else
            initialPose = new Pose2d(0.0f,0.0f,0);

        posReal.setOdometry(initialPose);

        Vector2d point;

        if(isSimulated)
            point = new Vector2d(15.0f,19.5f,Vector2d.MODE_XY);
        else
            point = new Vector2d(-0.5f,0.2f,Vector2d.MODE_XY);

        NxMSGoToPointNew goto2 = new NxMSGoToPointNew("goto",posReal,point, 4, 0.2f, NxMSGoToPoint.MODE_EXPONENTIAL);


        NxPSDetectObstacles obstReal;

        if(isSimulated)
            obstReal = new NxPSDetectObstaclesSonarNew("obstReal",posReal, ir, true  );
        else
            obstReal = new NxPSDetectObstaclesIRNew("obstReal",posReal, ir);

        NxPSMapObstaclesNew obstMapped = new NxPSMapObstaclesNew("6mapObstacles",obstReal, posReal,showMapObstFrame);
        
        NxPSMapObstaclesNew obstMapped2 = new NxPSMapObstaclesNew("mapObstacles",obstReal, posReal,0.3f,-1,showMapObstFrame);
        
        
        NxMSAvoidObstaclesNew avoidReal = new NxMSAvoidObstaclesNew("avoidReal",obstMapped, posReal,NxMSAvoidObstacles.MODE_EXPONENTIAL);


        avoidReal.setExponentialDistance(0.3f);

        NxMSGoForward forward = new NxMSGoForward("forward",posReal);

        NxMSNavigationVFHNew avoid = new NxMSNavigationVFHNew("avoidVFH",obstMapped,goto2,posReal,NxMSAvoidObstacles.MODE_LINEAR,true);

        NxMSNoise noise = new NxMSNoise("noise");
        
        NxPSMapVP vanishPoint;
        
        vanishPoint = new NxPSMapVP("vanish",vp,posReal);
        vanishPoint.setGain(0.1f);

        
        goto2.setGain(0.6f);
        avoidReal.setGain(0.8f);
        avoid.setGain(0.7f);
        noise.setGain(0.4f);


        forward.setGain(0.6f);

        Behavior[] array = {goto2,avoidReal};
        //Behavior[] array = {forward,vanishPoint};//,avoidReal};


        

        CooperativeArbitratorNew arbitrator1 = new CooperativeArbitratorNew("a1", array, showArbFrame);

        arbitrator1.enabled = true;
        


        
        
        try
        {
        Thread.sleep(2000);
        }
        catch(Exception e)
        {
            e.printStackTrace();
        }

        

        while(true)
        {
         
            Vector2d total = arbitrator1.Value();

            PlayerPose total1 = total.toPlayerPose();

            posH1.goTo(total1, posReal);

            int fidelity = 0;
            int oldFid = 0;
            int counts = 0;
            if(isUsingVp)
            {
                Vector2d newGoalPoint = vanishPoint.Value();
                fidelity = vanishPoint.getFidelity();

                if(newGoalPoint != null)
                {
                    System.out.println("Vector de puntoDesv:");
                    newGoalPoint.print(Vector2d.MODE_XY);
                    System.out.println("Posicion Robot:");
                    Position2DHelper.getPosition(posReal).print(Vector2d.MODE_XY);

                    System.out.println("fidelity: "+fidelity);

                    //if(fidelity > oldFid)
                    //{
                        goto2.changeGoalPoint(newGoalPoint);
                        //oldFid = fidelity;
                        //counts = 0;
                    //}
                    //else if (counts>100)
                    //{
                        goto2.changeGoalPoint(newGoalPoint);
                        oldFid = fidelity;
                        counts = 0;
                    //}
                    //else
                      //  counts++;

                }
            }
            try
            {
                Thread.sleep(50);
            }
            catch(Exception e)
            {}
        }
        
        /*
        
      
        while(true)
        {

            float[] result = null;
            while(result == null)
                result = ir.read(Sensor.READ_MODE_WAIT);


            
            int resultadoIR = 0;
            for(int i=0; i<result.length;i++)
            {
                if(result[i] == 0)
                {
                    switch(i)
                    {
                        case 0:
                            resultadoIR += 1;
                            break;
                        case 1:
                            resultadoIR += 2;
                            break;
                        case 2:
                            resultadoIR += 4;
                            break;          
                        case 3:
                            resultadoIR += 8;
                            break;          
                        case 4:
                            resultadoIR += 16;
                            break;           
                        case 5:
                            resultadoIR += 32;
                            break;                            
                    }
                }
                
                
            }
            
            System.out.println(resultadoIR);
            
            
            switch(resultadoIR)
            {
                case 1:
                case 2:
                case 3:
                case 6:
                case 7:
                case 15:
                case 31:
                    posReal.setSpeed(0, -0.8f);
                    break;
                
                case 4:
                case 8:
                case 12:
                case 30:
                
                    posReal.setSpeed(-0.3f,0.8f);
                    break;
                case 16:
                case 32:
                case 48:
                case 24:
                case 56:
                case 28:
                case 60:
                case 62:
                    posReal.setSpeed(0, 0.8f);
                    break;
                default:
                    posReal.setSpeed(0.3f, 0);
            }
        }*/
     
    }

}
